#include "face_detection_gpu_core.h"
#include<sys/time.h>

__constant__ int dc_image_width;
__constant__ int dc_image_height;
__constant__ int dc_image_width_step;
__constant__ int dc_sum_width;
__constant__ int dc_sum_height;
__constant__ int dc_sum_width_step;
__constant__ int dc_cascade_count;
__constant__ int dc_cascade_origin_window_size_width;
__constant__ int dc_cascade_origin_window_size_height;
__constant__ int dc_max_stage_count;
__constant__ int dc_max_face_map_width;
__constant__ int dc_max_face_map_height;
__constant__ int dc_n_factor;
__constant__ unsigned char dc_red;
__constant__ unsigned char dc_green;
__constant__ unsigned char dc_blue;

#define Max(x,y) (((x) > (y)) ? (x) : (y))
#define Min(x,y) (((x) < (y)) ? (x) : (y))

__global__ void integralCol(float *g_sum,float *g_sqsum, int width, int height) {
  float curSum = 0., curSqsum = 0., val;
	
  //Find location of row start
  int col = threadIdx.x+(blockIdx.x*blockDim.x);
  int sum_col_start = 1 * dc_sum_width_step + col + 1;

  if(col < width)
  {
      //fill in values
	for (int i = 1; i < height; i++)
	{
            g_sum[sum_col_start + i * dc_sum_width_step] += g_sum[sum_col_start + (i - 1) * dc_sum_width_step];
            g_sqsum[sum_col_start + i * dc_sum_width_step] += g_sqsum[sum_col_start + (i - 1) * dc_sum_width_step];
	}
  }
  return;
}

__global__ void integralRow(unsigned char *g_image, float *g_sum,float *g_sqsum, int width, int height) {
    unsigned char val = 0;
    float curSum = 0., curSqsum = 0.;
	
  //Each thread calculates a row of image, find the row (0 to width-1)
    int idx = blockIdx.x * blockDim.x + threadIdx.x;
    int row = threadIdx.x+(blockIdx.x*blockDim.x);
    int image_row_start = row * dc_image_width_step;
    int sum_row_start = (row + 1) * dc_sum_width_step + 1;
    int i;
    
    if(row < height) {
            //fill in values
        for (i = 0; i < width; i++) {
            val = g_image[image_row_start+i];
            curSum += val;
            curSqsum += (val*val);
            g_sum[sum_row_start + i] = curSum;
            g_sqsum[sum_row_start + i] = curSqsum;

        }
        
    }
    return;
}

__global__ void RunHaarCascadeClassifier(float *d_sum, float *d_sqsum, float *factors, 
                                         int *d_classifier_rect0_x, int *d_classifier_rect0_y, int *d_classifier_rect0_width, int *d_classifier_rect0_height, float *d_classifier_rect0_weight, 
                                         int *d_classifier_rect1_x, int *d_classifier_rect1_y, int *d_classifier_rect1_width, int *d_classifier_rect1_height, float *d_classifier_rect1_weight, 
                                         int *d_classifier_rect2_x, int *d_classifier_rect2_y, int *d_classifier_rect2_width, int *d_classifier_rect2_height, float *d_classifier_rect2_weight, 
                                         float *d_classifier_alpha0, float *d_classifier_alpha1, float *d_classifier_threshold, 
                                         float *d_stage_threshold, int *d_stage_count,
                                         float *classifier_weight_scale, int *d_step, int *d_real_win_size_width, int *d_real_win_size_height, 
                                         char *d_face_map) {
    int tx = threadIdx.x; //x
    int bdx = blockDim.x;
    int bx = blockIdx.x;  //y
    int gdx = gridDim.x;
    int by = blockIdx.y;  //factor

    int id = by * gdx * dc_max_face_map_width + bx * dc_max_face_map_width + tx;
    /*
    int *p_debug_msg = d_debug_msg + id;
    float *p_float_debug_msg1 = d_float_debug_msg1 + id;
    float *p_float_debug_msg2 = d_float_debug_msg2 + id;
    */

    char *p_face_map = d_face_map + id;
    
    float factor = *(factors + by);
    int step = *(d_step + by);
    
    int ptx = tx * step;
    int pty = bx * step;
    
    int real_win_size_width = *(d_real_win_size_width + by);
    int real_win_size_height = *(d_real_win_size_height + by);
    
    float *p_stage_threshold = d_stage_threshold;
    int *p_stage_count = d_stage_count;
    
    int p_offset = pty * dc_sum_width_step + ptx;

    int equ_rect_y = factor;
    int equ_rect_x = factor;
    int equ_rect_width = (dc_cascade_origin_window_size_width - 2) * factor;
    int equ_rect_height = (dc_cascade_origin_window_size_height - 2) * factor;

    float feature_weight_scale = *(classifier_weight_scale + by);
    float cascade_inv_window_area = feature_weight_scale;
    
    float *cascade_p0 = d_sum + equ_rect_y * dc_sum_width_step + equ_rect_x + p_offset;
    float *cascade_p1 = d_sum + equ_rect_y * dc_sum_width_step + equ_rect_x + equ_rect_width + p_offset;
    float *cascade_p2 = d_sum + (equ_rect_y + equ_rect_height) * dc_sum_width_step + equ_rect_x + p_offset;
    float *cascade_p3 = d_sum + (equ_rect_y + equ_rect_height) * dc_sum_width_step + equ_rect_x + equ_rect_width + p_offset;
    float *cascade_pq0 = d_sqsum + equ_rect_y * dc_sum_width_step + equ_rect_x + p_offset;
    float *cascade_pq1 = d_sqsum + equ_rect_y * dc_sum_width_step + equ_rect_x + equ_rect_width + p_offset;
    float *cascade_pq2 = d_sqsum + (equ_rect_y + equ_rect_height) * dc_sum_width_step + equ_rect_x + p_offset;
    float *cascade_pq3 = d_sqsum + (equ_rect_y + equ_rect_height) * dc_sum_width_step + equ_rect_x + equ_rect_width + p_offset;
    
    float mean;
    float var;

    int *p_classifier_rect0_x = d_classifier_rect0_x;
    int *p_classifier_rect0_y = d_classifier_rect0_y;
    int *p_classifier_rect0_width = d_classifier_rect0_width;
    int *p_classifier_rect0_height = d_classifier_rect0_height;
    float *p_classifier_rect0_weight = d_classifier_rect0_weight;
    float *feature_rect0_p0;
    float *feature_rect0_p1;
    float *feature_rect0_p2;
    float *feature_rect0_p3;

    int *p_classifier_rect1_x = d_classifier_rect1_x;
    int *p_classifier_rect1_y = d_classifier_rect1_y;
    int *p_classifier_rect1_width = d_classifier_rect1_width;
    int *p_classifier_rect1_height = d_classifier_rect1_height;
    float *p_classifier_rect1_weight = d_classifier_rect1_weight;
    float *feature_rect1_p0;
    float *feature_rect1_p1;
    float *feature_rect1_p2;
    float *feature_rect1_p3;

    int *p_classifier_rect2_x = d_classifier_rect2_x;
    int *p_classifier_rect2_y = d_classifier_rect2_y;
    int *p_classifier_rect2_width = d_classifier_rect2_width;
    int *p_classifier_rect2_height = d_classifier_rect2_height;
    float *p_classifier_rect2_weight = d_classifier_rect2_weight;
    float *feature_rect2_p0;
    float *feature_rect2_p1;
    float *feature_rect2_p2;
    float *feature_rect2_p3;

    float *p_classifier_alpha0 = d_classifier_alpha0;
    float *p_classifier_alpha1 = d_classifier_alpha1;
    float *p_classifier_threshold = d_classifier_threshold;

    int tr_x;
    int tr_y;
    int tr_width;
    int tr_height;
    float area0;
    float sum0;
    float feature_rect0_weight;
    float feature_rect1_weight;
    float feature_rect2_weight;
    float my_threshold;
    float my_sum_all;
    float sum_0;
    float sum_1;
    float sum_2;
    int i;
    int j;
    int n_reduction;
    int n_thread;

    float stage_sum;
    int stage_count;
    int next_stage;

    if(ptx + real_win_size_width >= dc_sum_width - 2 ||
       pty + real_win_size_height >= dc_sum_height - 2) {
        *p_face_map = -127;
        return;
    }

    mean = *cascade_p0 - *cascade_p1 - *cascade_p2 + *cascade_p3; //mean of the patch
    var = *cascade_pq0 - *cascade_pq1 - *cascade_pq2 + *cascade_pq3; //var of the patch

    mean *= cascade_inv_window_area;

    var = var * cascade_inv_window_area - mean * mean;
    if(var < 0)
        var = 1;
    var = sqrt(var);

    for(i = 0; i < dc_cascade_count; i++) {
        stage_count = *p_stage_count;
        stage_sum = 0;
        for(j = 0; j < stage_count; j++) {
            tr_x = *p_classifier_rect0_x * factor + 0.5;
            tr_width = *p_classifier_rect0_width * factor + 0.5;
            tr_y = *p_classifier_rect0_y * factor + 0.5;
            tr_height = *p_classifier_rect0_height * factor + 0.5;
            feature_rect0_p0 = d_sum + tr_y * dc_sum_width + tr_x + p_offset;
            feature_rect0_p1 = d_sum + tr_y * dc_sum_width + tr_x + tr_width + p_offset;
            feature_rect0_p2 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + p_offset;
            feature_rect0_p3 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + tr_width + p_offset;
            area0 = tr_width * tr_height;
            
            tr_x = *p_classifier_rect1_x * factor + 0.5;
            tr_width = *p_classifier_rect1_width * factor + 0.5;
            tr_y = *p_classifier_rect1_y * factor + 0.5;
            tr_height = *p_classifier_rect1_height * factor + 0.5;
            feature_rect1_p0 = d_sum + tr_y * dc_sum_width + tr_x + p_offset;
            feature_rect1_p1 = d_sum + tr_y * dc_sum_width + tr_x + tr_width + p_offset;
            feature_rect1_p2 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + p_offset;
            feature_rect1_p3 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + tr_width + p_offset;
            feature_rect1_weight = *p_classifier_rect1_weight *  feature_weight_scale;
            sum0 = feature_rect1_weight * tr_width * tr_height;
            
            tr_x = *p_classifier_rect2_x * factor + 0.5;
            tr_width = *p_classifier_rect2_width * factor + 0.5;
            tr_y = *p_classifier_rect2_y * factor + 0.5;
            tr_height = *p_classifier_rect2_height * factor + 0.5;
            feature_rect2_p0 = d_sum + tr_y * dc_sum_width + tr_x + p_offset;
            feature_rect2_p1 = d_sum + tr_y * dc_sum_width + tr_x + tr_width + p_offset;
            feature_rect2_p2 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + p_offset;
            feature_rect2_p3 = d_sum + (tr_y + tr_height) * dc_sum_width + tr_x + tr_width + p_offset;
            feature_rect2_weight = *p_classifier_rect2_weight *  feature_weight_scale;
            sum0 += feature_rect2_weight * tr_width * tr_height;
            
            feature_rect0_weight = -sum0 / area0;
            
            my_threshold = *p_classifier_threshold * var;
            sum_0 = (*feature_rect0_p0 - *feature_rect0_p1 - *feature_rect0_p2 + *feature_rect0_p3) * feature_rect0_weight;
            sum_1 = (*feature_rect1_p0 - *feature_rect1_p1 - *feature_rect1_p2 + *feature_rect1_p3) * feature_rect1_weight;
            sum_2 = (*feature_rect2_p0 - *feature_rect2_p1 - *feature_rect2_p2 + *feature_rect2_p3) * feature_rect2_weight;
            my_sum_all = sum_0 + sum_1 + sum_2;
            
            if(my_sum_all >= my_threshold)
                stage_sum += *p_classifier_alpha1;
            else
                stage_sum += *p_classifier_alpha0;
            p_classifier_rect0_x++;
            p_classifier_rect0_y++;
            p_classifier_rect0_width++;
            p_classifier_rect0_height++;
            p_classifier_rect0_weight++;
            
            p_classifier_rect1_x++;
            p_classifier_rect1_y++;
            p_classifier_rect1_width++;
            p_classifier_rect1_height++;
            p_classifier_rect1_weight++;
            
            p_classifier_rect2_x++;
            p_classifier_rect2_y++;
            p_classifier_rect2_width++;
            p_classifier_rect2_height++;
            p_classifier_rect2_weight++;
            
            p_classifier_alpha0++;
            p_classifier_alpha1++;
            p_classifier_threshold++;
        }
        
        if(stage_sum < *p_stage_threshold) {
            *p_face_map = -(i + 1);
            return;
        }
        
        //to next pointer
        next_stage = dc_max_stage_count - stage_count;
        p_classifier_rect0_x += next_stage;
        p_classifier_rect0_y += next_stage;
        p_classifier_rect0_width += next_stage;
        p_classifier_rect0_height += next_stage;
        p_classifier_rect0_weight += next_stage;
        
        p_classifier_rect1_x += next_stage;
        p_classifier_rect1_y += next_stage;
        p_classifier_rect1_width += next_stage;
        p_classifier_rect1_height += next_stage;
        p_classifier_rect1_weight += next_stage;
        
        p_classifier_rect2_x += next_stage;
        p_classifier_rect2_y += next_stage;
        p_classifier_rect2_width += next_stage;
        p_classifier_rect2_height += next_stage;
        p_classifier_rect2_weight += next_stage;
        
        p_classifier_alpha0 += next_stage;
        p_classifier_alpha1 += next_stage;
        p_classifier_threshold += next_stage;
        
        p_stage_threshold++;
        p_stage_count++;
    }
    *p_face_map = 1;
    return;
}

__global__ void DrawFaceOnImage(unsigned char *d_image_data, char *d_face_map, 
                                float *factors, int *d_real_win_size_width, int *d_real_win_size_height, int *d_step) {
                                
    int tx = threadIdx.x;
    int bdx = blockDim.x;
    int bx = blockIdx.x;
    int gdx = gridDim.x;
    int by = blockIdx.y % dc_max_face_map_height;
    int gdy = dc_max_face_map_height;
    int bz = blockIdx.y / dc_max_face_map_height;

    int factor_offset = bz;
    float factor = *(factors + factor_offset);
    int real_win_size_width = *(d_real_win_size_width + factor_offset);
    int real_win_size_height = *(d_real_win_size_height + factor_offset);
    int step = *(d_step + factor_offset);
    int ptx = bx * step;
    int pty = by * step;

    if(tx >= real_win_size_width)
        return;

    if(tx >= real_win_size_height)
        return;

    int idx_face_map = bz * gdy * gdx + by * gdx + bx;
    char face_map = *(d_face_map + idx_face_map);
    /*
    int *p_debug_msg = d_debug_msg + idx_face_map;
    float *p_float_debug_msg1 = d_float_debug_msg1 + idx_face_map;
    */

    if(face_map < 0)
        return;

    //top
    int draw_id = tx;
    int idx_image_data = (pty * dc_image_width_step + ptx + tx) * 3;
    unsigned char *p_image_data = d_image_data + idx_image_data;
    while(draw_id < real_win_size_width) {
        *p_image_data = dc_blue;
        *(p_image_data + 1) = dc_green;
        *(p_image_data + 2) + dc_red;

        draw_id += bdx;
        p_image_data += bdx * 3;
    }

    //left
    draw_id = tx;
    idx_image_data = ((pty + draw_id) * dc_image_width_step + ptx) * 3;
    p_image_data = d_image_data + idx_image_data;
    while(draw_id < real_win_size_height) {
        *p_image_data = dc_blue;
        *(p_image_data + 1) = dc_green;
        *(p_image_data + 2) = dc_red;

        draw_id += bdx;
        p_image_data += dc_image_width_step * bdx * 3;
    }

    //right
    draw_id = tx;
    idx_image_data = ((pty + draw_id) * dc_image_width_step + ptx + real_win_size_width - 1) * 3;
    p_image_data = d_image_data + idx_image_data;
    while(draw_id < real_win_size_height) {
        *p_image_data = dc_blue;
        *(p_image_data + 1) = dc_green;
        *(p_image_data + 2) = dc_red;

        draw_id += bdx;
        p_image_data += dc_image_width_step * bdx * 3;
    }

    //bottom
    draw_id = tx;
    idx_image_data = ((pty + real_win_size_height - 1) * dc_image_width_step + ptx + draw_id) * 3;
    p_image_data = d_image_data + idx_image_data;
    while(draw_id < real_win_size_height) {
        *p_image_data = dc_blue;
        *(p_image_data + 1) = dc_green;
        *(p_image_data + 2) = dc_red;

        draw_id += bdx;
        p_image_data += bdx * 3;
    }
}

void FaceDetectionGPUCore(int img_cols, int img_rows, int img_width_step, int sum_width, int sum_height, int sum_width_step,
                          int cascade_origin_window_size_width, int cascade_origin_window_size_height, 
                          float scale_factor, int cascade_count, int max_stage_count, 
                          unsigned char *image_data, float *sum, float *sqsum, int *stage_count, float *stage_threshold,
                          int *classifier_rect0_x, int *classifier_rect0_y, int *classifier_rect0_width, int *classifier_rect0_height, float *classifier_rect0_weight, 
                          int *classifier_rect1_x, int *classifier_rect1_y, int *classifier_rect1_width, int *classifier_rect1_height, float *classifier_rect1_weight, 
                          int *classifier_rect2_x, int *classifier_rect2_y, int *classifier_rect2_width, int *classifier_rect2_height, float *classifier_rect2_weight, 
                          float *classifier_alpha0, float *classifier_alpha1, float *classifier_threshold) {
    fprintf(stderr, "FaceDetectionGPUCore: img_cols (width): %d img_rows (height): %d sum: (%d, %d) origin_window_size: (%d, %d) scale_factor: %f cascade_count: %d max_stage_count: %d\n", 
            img_cols, img_rows, 
            sum_width, sum_height, 
            cascade_origin_window_size_width, cascade_origin_window_size_height, 
            scale_factor, cascade_count, max_stage_count);
    //0 ~ log()
int n_factor_by_width = log((double)(img_cols - 10) / cascade_origin_window_size_width) / log(scale_factor) + 1;
//0 ~ log()
    int n_factor_by_height = log((double)(img_rows - 10) / cascade_origin_window_size_height) / log(scale_factor) + 1;    
    int n_factor = Min(n_factor_by_width, n_factor_by_height);

    /*
    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: n_factor: (%d, %d, %d)\n", n_factor_by_width, n_factor_by_height, n_factor);
 
    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: sum: \n");
    for(int i = 0; i < sum_height; i++) {
        for(int j = 0; j < sum_width; j++)
            fprintf(stderr, "%6.0f ", *(sum + i * sum_width + j));
        fprintf(stderr, "\n");
    }
    */

    /*********************
     * mem size
     *********************/
    int max_face_map_width = (img_cols - cascade_origin_window_size_width) / 2 + 1;
    int max_face_map_height = (img_rows - cascade_origin_window_size_height) / 2 + 1;

    int image_mem_size = img_cols * img_rows * 3 * sizeof(unsigned char);
    int sum_mem_size = sum_width * sum_height * sizeof(float);

    int sqsum_mem_size = sum_mem_size;
    int factors_mem_size = n_factor * sizeof(float);

    int classifier_rect0_x_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect0_y_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect0_width_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect0_height_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect0_weight_mem_size = cascade_count * max_stage_count * sizeof(float);

    int classifier_rect1_x_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect1_y_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect1_width_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect1_height_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect1_weight_mem_size = cascade_count * max_stage_count * sizeof(float);

    int classifier_rect2_x_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect2_y_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect2_width_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect2_height_mem_size = cascade_count * max_stage_count * sizeof(int);
    int classifier_rect2_weight_mem_size = cascade_count * max_stage_count * sizeof(float);

    int classifier_alpha0_mem_size = cascade_count * max_stage_count * sizeof(float);
    int classifier_alpha1_mem_size = cascade_count * max_stage_count * sizeof(float);
    int classifier_threshold_mem_size = cascade_count * max_stage_count * sizeof(float);

    int stage_threshold_mem_size = cascade_count * sizeof(float);
    int stage_count_mem_size = cascade_count * sizeof(int);

    int classifier_weight_scale_mem_size = n_factor * sizeof(float);
    int step_mem_size = n_factor * sizeof(int);
    int real_win_size_width_mem_size = n_factor * sizeof(int);
    int real_win_size_height_mem_size = n_factor * sizeof(int);

    int face_map_mem_size = n_factor * max_face_map_width * max_face_map_height * sizeof(char);

    int debug_msg_mem_size = n_factor * max_face_map_width * max_face_map_height * sizeof(int);
    int float_debug_msg_mem_size = n_factor * max_face_map_width * max_face_map_height * sizeof(float);
    
    /*********************
     * cpu var
     *********************/
    unsigned char red = 255;
    unsigned char green = 0;
    unsigned char blue = 0;
    float *factors = (float *)malloc(factors_mem_size);
    
    float *classifier_weight_scale = (float *)malloc(classifier_weight_scale_mem_size);
    int *step = (int *)malloc(step_mem_size);
    int *real_win_size_width = (int *)malloc(real_win_size_width_mem_size);
    int *real_win_size_height = (int *)malloc(real_win_size_height_mem_size);

    int equ_rect_width;
    int equ_rect_height;
    double temp_double;
    int max_win_length = 0;
    for(int i = 0; i < n_factor; i++) { //for each scale factor
        if(i == 0)
            factors[i] = 1;
        else
            factors[i] = factors[i - 1] * scale_factor;

        step[i] = Max(2, factors[i]);
        real_win_size_width[i] = cascade_origin_window_size_width * factors[i]; //real win size width
        real_win_size_height[i] = cascade_origin_window_size_height * factors[i]; //real win size height
        equ_rect_width = (cascade_origin_window_size_width - 2 + 0.5) * factors[i];
        equ_rect_height = (cascade_origin_window_size_height - 2) * factors[i]; 
        max_win_length = Max(max_win_length, real_win_size_width[i]);
        max_win_length = Max(max_win_length, real_win_size_height[i]);
        temp_double = 1. /(equ_rect_width * equ_rect_height);
        classifier_weight_scale[i] = temp_double;
    }

    char *face_map = (char*)malloc(face_map_mem_size);

    /*
    int *debug_msg = (int *)malloc(debug_msg_mem_size);
    float *float_debug_msg1 = (float *)malloc(float_debug_msg_mem_size);
    */
    /*
    float *float_debug_msg2 = (float *)malloc(float_debug_msg_mem_size);
    */

    /*********************
     * declare var
     *********************/

    unsigned char *d_image_data;
    float *d_sum;
    float *d_sqsum;
    float *d_factors;

    int *d_classifier_rect0_x;
    int *d_classifier_rect0_y;
    int *d_classifier_rect0_width;
    int *d_classifier_rect0_height;
    float *d_classifier_rect0_weight;

    int *d_classifier_rect1_x;
    int *d_classifier_rect1_y;
    int *d_classifier_rect1_width;
    int *d_classifier_rect1_height;
    float *d_classifier_rect1_weight;

    int *d_classifier_rect2_x;
    int *d_classifier_rect2_y;
    int *d_classifier_rect2_width;
    int *d_classifier_rect2_height;
    float *d_classifier_rect2_weight;

    float *d_classifier_alpha0;
    float *d_classifier_alpha1;
    float *d_classifier_threshold;

    float *d_stage_threshold;
    int *d_stage_count;

    float *d_classifier_weight_scale;
    int *d_step;
    int *d_real_win_size_width;
    int *d_real_win_size_height;

    char *d_face_map;

    /*
    int *d_debug_msg;
    float *d_float_debug_msg1;
    */
    /*
    float *d_float_debug_msg2;
    */
    
    /*********************
     * alloc var
     *********************/
    cudaMalloc((void **)&d_image_data, image_mem_size);
    cudaMalloc((void **)&d_sum, sum_mem_size);
    cudaMalloc((void **)&d_sqsum, sqsum_mem_size);
    cudaMalloc((void **)&d_factors, factors_mem_size);

    cudaMalloc((void **)&d_classifier_rect0_x, classifier_rect0_x_mem_size);
    cudaMalloc((void **)&d_classifier_rect0_y, classifier_rect0_y_mem_size);
    cudaMalloc((void **)&d_classifier_rect0_width, classifier_rect0_width_mem_size);
    cudaMalloc((void **)&d_classifier_rect0_height, classifier_rect0_height_mem_size);
    cudaMalloc((void **)&d_classifier_rect0_weight, classifier_rect0_weight_mem_size);
    
    cudaMalloc((void **)&d_classifier_rect1_x, classifier_rect1_x_mem_size);
    cudaMalloc((void **)&d_classifier_rect1_y, classifier_rect1_y_mem_size);
    cudaMalloc((void **)&d_classifier_rect1_width, classifier_rect1_width_mem_size);
    cudaMalloc((void **)&d_classifier_rect1_height, classifier_rect1_height_mem_size);
    cudaMalloc((void **)&d_classifier_rect1_weight, classifier_rect1_weight_mem_size);

    cudaMalloc((void **)&d_classifier_rect2_x, classifier_rect2_x_mem_size);
    cudaMalloc((void **)&d_classifier_rect2_y, classifier_rect2_y_mem_size);
    cudaMalloc((void **)&d_classifier_rect2_width, classifier_rect2_width_mem_size);
    cudaMalloc((void **)&d_classifier_rect2_height, classifier_rect2_height_mem_size);
    cudaMalloc((void **)&d_classifier_rect2_weight, classifier_rect2_weight_mem_size);

    cudaMalloc((void **)&d_classifier_alpha0, classifier_alpha0_mem_size);
    cudaMalloc((void **)&d_classifier_alpha1, classifier_alpha1_mem_size);
    cudaMalloc((void **)&d_classifier_threshold, classifier_threshold_mem_size);
    
    cudaMalloc((void **)&d_stage_threshold, stage_threshold_mem_size);
    cudaMalloc((void **)&d_stage_count, stage_count_mem_size);

    cudaMalloc((void **)&d_classifier_weight_scale, classifier_weight_scale_mem_size);
    cudaMalloc((void **)&d_step, step_mem_size);
    cudaMalloc((void **)&d_real_win_size_width, real_win_size_width_mem_size);
    cudaMalloc((void **)&d_real_win_size_height, real_win_size_height_mem_size);

    cudaMalloc((void **)&d_face_map, face_map_mem_size);

    /*
    cudaMalloc((void **)&d_debug_msg, debug_msg_mem_size);
    cudaMalloc((void **)&d_float_debug_msg1, float_debug_msg_mem_size);
    */
    /*
    cudaMalloc((void **)&d_float_debug_msg2, float_debug_msg_mem_size);
    */

    /*********************
     * init var
     *********************/
    cudaMemcpyToSymbol("dc_image_width", &img_cols, sizeof(int));
    cudaMemcpyToSymbol("dc_image_height", &img_rows, sizeof(int));
    cudaMemcpyToSymbol("dc_image_width_step", &img_width_step, sizeof(int));
    cudaMemcpyToSymbol("dc_sum_width", &sum_width, sizeof(int));
    cudaMemcpyToSymbol("dc_sum_height", &sum_height, sizeof(int));
    cudaMemcpyToSymbol("dc_sum_width_step", &sum_width_step, sizeof(int));
    cudaMemcpyToSymbol("dc_cascade_count", &cascade_count, sizeof(int));
    cudaMemcpyToSymbol("dc_cascade_origin_window_size_width", &cascade_origin_window_size_width, sizeof(int));
    cudaMemcpyToSymbol("dc_cascade_origin_window_size_height", &cascade_origin_window_size_height, sizeof(int));
    cudaMemcpyToSymbol("dc_max_stage_count", &max_stage_count, sizeof(int));
    cudaMemcpyToSymbol("dc_max_face_map_width", &max_face_map_width, sizeof(int));
    cudaMemcpyToSymbol("dc_max_face_map_height", &max_face_map_height, sizeof(int));
    cudaMemcpyToSymbol("dc_n_factor", &n_factor, sizeof(int));
    cudaMemcpyToSymbol("dc_red", &red, sizeof(unsigned char));
    cudaMemcpyToSymbol("dc_green", &green, sizeof(unsigned char));
    cudaMemcpyToSymbol("dc_blue", &blue, sizeof(unsigned char));

    cudaMemcpy(d_image_data, image_data, image_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_sum, sum, sum_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_sqsum, sqsum, sqsum_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_factors, factors, factors_mem_size, cudaMemcpyHostToDevice);

    cudaMemcpy(d_classifier_rect0_x, classifier_rect0_x, classifier_rect0_x_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect0_y, classifier_rect0_y, classifier_rect0_y_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect0_width, classifier_rect0_width, classifier_rect0_width_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect0_height, classifier_rect0_height, classifier_rect0_height_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect0_weight, classifier_rect0_weight, classifier_rect0_weight_mem_size, cudaMemcpyHostToDevice);
    
    cudaMemcpy(d_classifier_rect1_x, classifier_rect1_x, classifier_rect1_x_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect1_y, classifier_rect1_y, classifier_rect1_y_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect1_width, classifier_rect1_width, classifier_rect1_width_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect1_height, classifier_rect1_height, classifier_rect1_height_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect1_weight, classifier_rect1_weight, classifier_rect1_weight_mem_size, cudaMemcpyHostToDevice);

    cudaMemcpy(d_classifier_rect2_x, classifier_rect2_x, classifier_rect2_x_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect2_y, classifier_rect2_y, classifier_rect2_y_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect2_width, classifier_rect2_width, classifier_rect2_width_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect2_height, classifier_rect2_height, classifier_rect2_height_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_rect2_weight, classifier_rect2_weight, classifier_rect2_weight_mem_size, cudaMemcpyHostToDevice);

    cudaMemcpy(d_classifier_alpha0, classifier_alpha0, classifier_alpha0_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_alpha1, classifier_alpha1, classifier_alpha1_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_classifier_threshold, classifier_threshold, classifier_threshold_mem_size, cudaMemcpyHostToDevice);
    
    cudaMemcpy(d_stage_threshold, stage_threshold, stage_threshold_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_stage_count, stage_count, stage_count_mem_size, cudaMemcpyHostToDevice);

    cudaMemcpy(d_classifier_weight_scale, classifier_weight_scale, classifier_weight_scale_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_step, step, step_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_real_win_size_width, real_win_size_width, real_win_size_width_mem_size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_real_win_size_height, real_win_size_height, real_win_size_height_mem_size, cudaMemcpyHostToDevice);

    cudaMemset(d_face_map, 0, face_map_mem_size);
    /*
    cudaMemset(d_debug_msg, 0, debug_msg_mem_size);
    cudaMemset(d_float_debug_msg1, 0, float_debug_msg_mem_size);
    */
    /*
    cudaMemset(d_float_debug_msg2, 0, float_debug_msg_mem_size);
    */

    /*************************
     * main procedure
     *************************/
    dim3 grid_dim, block_dim;
    
    grid_dim.x = max_face_map_height;
    grid_dim.y = n_factor;
    block_dim.x = Min(CUDA_MAX_THREAD, max_face_map_width);

    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: max_face_map_width: %d max_face_map_height: %d n_factor: %d grid_dim: (%d, %d) block_dim: %d\n", 
            max_face_map_width, max_face_map_height, n_factor, 
            grid_dim.x, grid_dim.y, block_dim.x);

    struct timeval start_tv, end_tv;
    struct timezone start_tz, end_tz;
    int total_sec, total_usec;

    gettimeofday(&start_tv, &start_tz);
    RunHaarCascadeClassifier<<<grid_dim, block_dim>>>
        (d_sum, d_sqsum, d_factors, 
         d_classifier_rect0_x, d_classifier_rect0_y, d_classifier_rect0_width, d_classifier_rect0_height, d_classifier_rect0_weight, 
         d_classifier_rect1_x, d_classifier_rect1_y, d_classifier_rect1_width, d_classifier_rect1_height, d_classifier_rect1_weight, 
         d_classifier_rect2_x, d_classifier_rect2_y, d_classifier_rect2_width, d_classifier_rect2_height, d_classifier_rect2_weight, 
         d_classifier_alpha0, d_classifier_alpha1, d_classifier_threshold, 
         d_stage_threshold, d_stage_count,
         d_classifier_weight_scale, d_step, d_real_win_size_width, d_real_win_size_height, 
         d_face_map);

    //fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: after RunHaarCascadeClassifier: err_str: %s\n", cudaGetErrorString(cudaGetLastError()));

    /*************************
     * post main procedure
     *************************/

    cudaThreadSynchronize();
    gettimeofday(&end_tv, &end_tz);
    total_sec = end_tv.tv_sec - start_tv.tv_sec;
    total_usec = end_tv.tv_usec - start_tv.tv_usec;
    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: after RunHaarCascadeClassifier: time: %d.%06d\n", total_sec, total_usec);

    cudaMemcpy(face_map, d_face_map, face_map_mem_size, cudaMemcpyDeviceToHost);


    //cudaMemcpy(debug_msg, d_debug_msg, debug_msg_mem_size, cudaMemcpyDeviceToHost);
    //cudaMemcpy(float_debug_msg1, d_float_debug_msg1, float_debug_msg_mem_size, cudaMemcpyDeviceToHost);
    //cudaMemcpy(float_debug_msg2, d_float_debug_msg2, float_debug_msg_mem_size, cudaMemcpyDeviceToHost);

    /*
    fprintf(stderr, "float_debug_msg_all: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "(%.3f, %.3f) ", 
                        float_debug_msg1[k * max_face_map_height * max_face_map_width + i * max_face_map_width + j],
                        float_debug_msg2[k * max_face_map_height * max_face_map_width + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /*
    fprintf(stderr, "float_debug_msg1: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "%.3f ", float_debug_msg1[k * max_face_map_height * max_face_map_width + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /*
    fprintf(stderr, "float_debug_msg2: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "%.3f ", float_debug_msg2[k * max_face_map_height * max_face_map_width + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /*
    fprintf(stderr, "int_debug_msg: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "%d ", debug_msg[k * max_face_map_width * max_face_map_height + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /*
    fprintf(stderr, "face_map: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "%d ", face_map[k * max_face_map_width * max_face_map_height + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /**************
     * draw face 
     **************/
    grid_dim.x = max_face_map_width;
    grid_dim.y = max_face_map_height * n_factor;
    block_dim.x = Min(CUDA_MAX_THREAD, max_win_length);

    DrawFaceOnImage<<<grid_dim, block_dim>>>(d_image_data, d_face_map, 
                                             d_factors, d_real_win_size_width, d_real_win_size_height, d_step);

    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: after DrawFaceOnImage: err_str: %s\n", cudaGetErrorString(cudaGetLastError()));

    /**************
     * post draw face 
     **************/
    
    //cudaMemcpy(debug_msg, d_debug_msg, debug_msg_mem_size, cudaMemcpyDeviceToHost);
    cudaMemcpy(image_data, d_image_data, image_mem_size, cudaMemcpyDeviceToHost);


    /*
    fprintf(stderr, "int_debug_msg: \n");
    for(int k = 0; k < n_factor; k++) {
        fprintf(stderr, "factor: %.3f\n", factors[k]);
        for(int i = 0; i < max_face_map_height; i++) {
            fprintf(stderr, "  ");
            for(int j = 0; j < max_face_map_width; j++)
                fprintf(stderr, "%d ", debug_msg[k * max_face_map_width * max_face_map_height + i * max_face_map_width + j]);
            fprintf(stderr, "\n");
        }
    }
    */

    /**************
     * release 
     **************/
    //gpu
    cudaFree(d_image_data);
    cudaFree(d_sum);
    cudaFree(d_sqsum);
    cudaFree(d_factors);

    cudaFree(d_classifier_rect0_x);
    cudaFree(d_classifier_rect0_y);
    cudaFree(d_classifier_rect0_width);
    cudaFree(d_classifier_rect0_height);
    cudaFree(d_classifier_rect0_weight);
    
    cudaFree(d_classifier_rect1_x);
    cudaFree(d_classifier_rect1_y);
    cudaFree(d_classifier_rect1_width);
    cudaFree(d_classifier_rect1_height);
    cudaFree(d_classifier_rect1_weight);

    cudaFree(d_classifier_rect2_x);
    cudaFree(d_classifier_rect2_y);
    cudaFree(d_classifier_rect2_width);
    cudaFree(d_classifier_rect2_height);
    cudaFree(d_classifier_rect2_weight);

    cudaFree(d_classifier_alpha0);
    cudaFree(d_classifier_alpha1);
    cudaFree(d_classifier_threshold);
    
    cudaFree(d_stage_threshold);
    cudaFree(d_stage_count);

    cudaFree(d_classifier_weight_scale);
    cudaFree(d_step);
    cudaFree(d_real_win_size_width);
    cudaFree(d_real_win_size_height);

    cudaFree(d_face_map);

    /*
    cudaFree(d_debug_msg);
    cudaFree(d_float_debug_msg1);
    cudaFree(d_float_debug_msg2);
    */

    //cpu
    free(factors);

    free(classifier_weight_scale);
    free(step);
    free(real_win_size_width);
    free(real_win_size_height);

    free(face_map);

    /*
    free(debug_msg);
    free(float_debug_msg1);
    free(float_debug_msg2);
    */
}

void GPUIntegralImage(unsigned char* h_image,float* h_sum,float* h_sqsum, int width, int height, int image_width_step, int sum_width_step)
{

    fprintf(stderr, "face_detection_gpu_core_hsiao::GPUIntegralImage: image_width_step: %d sum_width_step: %d\n", image_width_step, sum_width_step);
    cudaMemcpyToSymbol("dc_image_width_step", &image_width_step, sizeof(int));
    cudaMemcpyToSymbol("dc_sum_width_step", &sum_width_step, sizeof(int));
    
    unsigned char* d_image = NULL;
    float* d_sum = NULL;
    float* d_sqsum = NULL;
    int sum_width = sum_width_step;
    int sum_height = height + 1;
    struct timeval start_tv, end_tv;
    struct timezone start_tz, end_tz;
    int total_sec, total_usec;

    fprintf(stderr, "face_detection_gpu_integral: GPUIntegralImage: (%d, %d)\n", width, height);
    // Memory allocation on device side
    cudaMalloc((void**)&d_image, image_width_step*height*sizeof(unsigned char));
    cudaMalloc((void**)&d_sum, sum_width*sum_height*sizeof(float));
    cudaMalloc((void**)&d_sqsum, sum_width*sum_height*sizeof(float));
    cudaMemset(d_sum, 0, sum_width*sum_height*sizeof(float));
    cudaMemset(d_sqsum, 0, sum_width*sum_height*sizeof(float));

    // Copy from host memory to device memory
    cudaMemcpy(d_image, h_image, image_width_step*height*sizeof(unsigned char), cudaMemcpyHostToDevice);

    // Invoke the kernel
    //Each thread does a row
    dim3 block,rowGrid,colGrid;
    block.x = 512;
    block.y = block.z = 1;
    rowGrid.x = static_cast<int>( ceil( static_cast<float>(height) / block.x ) );
    rowGrid.y = rowGrid.z = 1;
    colGrid.x = static_cast<int>( ceil( static_cast<float>(width) / block.x ) );
    colGrid.y = colGrid.z = 1;

    gettimeofday(&start_tv, &start_tz);
    integralRow<<<rowGrid,block>>>(d_image, d_sum, d_sqsum, width, height);

    //fprintf(stderr, "face_detection_gpu_core_hsiao::GPUIntegralImage: after integralRow: err_str: %s\n", cudaGetErrorString(cudaGetLastError()));

    integralCol<<<colGrid,block>>>(d_sum, d_sqsum, width, height);

    //fprintf(stderr, "face_detection_gpu_core_hsiao::GPUIntegralImage: after integralCol: err_str: %s\n", cudaGetErrorString(cudaGetLastError()));

    cudaThreadSynchronize();
    gettimeofday(&end_tv, &end_tz);
    total_sec = end_tv.tv_sec - start_tv.tv_sec;
    total_usec = end_tv.tv_usec - start_tv.tv_usec;
    fprintf(stderr, "face_detection_gpu_core_hsiao::FaceDetectionGPUCore: after GPUIntegralImage: time: %d.%06d\n", total_sec, total_usec);

    // Copy from device memory back to host memory
    cudaMemcpy(h_sum, d_sum, sum_width*sum_height*sizeof(float), cudaMemcpyDeviceToHost);
    cudaMemcpy(h_sqsum, d_sqsum, sum_width*sum_height*sizeof(float), cudaMemcpyDeviceToHost);

    /*
    fprintf(stderr, "face_detection_gpu_integral_image::GPUIntegralImage: image: \n");
    for(int i = 0; i < height; i++) {
        for(int j = 0; j < width; j++)
            fprintf(stderr, "%d ", h_image[i * image_width_step + j]);
        fprintf(stderr, "\n");
    }

    fprintf(stderr, "face_detection_gpu_integral_image::GPUIntegralImage: sum: \n");
    for(int i = 0; i < sum_height; i++) {
        for(int j = 0; j < sum_width; j++)
            fprintf(stderr, "%.3f ", h_sum[i * sum_width_step + j]);
        fprintf(stderr, "\n");
    }

    fprintf(stderr, "face_detection_gpu_integral_image::GPUIntegralImage: sqsum: \n");
    for(int i = 0; i < sum_height; i++) {
        for(int j = 0; j < sum_width; j++)
            fprintf(stderr, "%.3f ", h_sqsum[i * sum_width_step + j]);
        fprintf(stderr, "\n");
    }
    */

    cudaFree(d_sum);
    cudaFree(d_sqsum);
    cudaFree(d_image);
    fprintf(stderr, " GPUIntegralImage done, return to cpu \n");
}


